% -*-coding: utf-8 -*-

\defaultfont
\appendix

\renewcommand{\appendixname}{附录~\Alph{chapter}}

\setcounter{equation}{0}

\renewcommand\theequation{A-\arabic{equation}}
\renewcommand\thesection{A.\arabic{section}}

\setcounter{algocf}{0}
\renewcommand{\thealgocf}{A-\arabic{algocf}}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\BiAppChapter{附录~A~ 协同定位的估计引擎}{Appendix~A~ Estimation Engines for CL} \label{ch5sec. appendix}


协同定位的目的是通过处理协同系统中收集到的通信和观测数据，估计系统中所有节点的位姿信息。因此协同定位任务是典型的状态估计问题。目前在定位任务的背景下，此类状态估计问题的求解方法主要有两种，即
\begin{itemize}
	\item[(1)] 基于马尔可夫假设的递归贝叶斯滤波方法。此类方法假设系统的时序状态之间具有相互独立的“无记忆”马尔可夫性质，即系统在下一时刻的状态只能由当前状态和控制输入决定，与过去和未来的状态无关。因此，此类滤波方法可以使用贝叶斯定理和全概率公式，通过递归的方式估计每一时刻的系统状态。在每一次递归过程中，由于前一时刻的系统状态已知，因此估计方法仅需处理并且得到当前时刻的状态估计值。此类方法的典型代表是卡尔曼滤波，拓展卡尔曼滤波以及粒子滤波等方法。
	\item[(2)] 基于非线性最小二乘的最大后验概率平滑方法。与贝叶斯滤波不同的是，此类方法将系统现在和过去时刻所有的状态共同作为估计量，根据所有历史测量数据，同时估计系统在现在和过去时刻的全部轨迹。一类典型的求解过程是将历史数据中的每一次测量转换为信念网络 (belief network) 中的一个节点因子 (factor), 然后通过优化整个信念网络的最大后验概率，得出系统历史轨迹整体的平滑估计。当系统模型采用高斯噪声时，求解最大后验概率的过程可以转变为求解一类非线性最小二乘问题。
\end{itemize}

为了更好地区分上述两种方法，后文将它们称为状态估计问题的求解引擎，分别表述为“滤波引擎”和“平滑引擎”。
多智能体协同定位问题是在已知系统运动模型和历史测量数据的基础上，寻找优化的$\vect{\bar{X}}^{0:k}$以及$\vect{\Sigma}^k_{\vect{\bar{X}}^{0:k}}$，以匹配如下的联合信念，即后验概率密度函数)：
\begin{equation} \label{eq. ACTCO_MAP}
	\begin{aligned}
		b(\vect{X}^{0:k}) &= p(\vect{X}^{0:k}|\vect{Z}^{0:k},\vect{U}^{0:k-1}) \\
		&= \prod_{i \in \mathcal{V}_R} p(\vect{p}_i^0) \prod_{t=1}^{k} \left[ p(\vect{p}_{i}^{t}|\vect{p}_i^{t-1},\vect{u}_i^{t-1}) \prod_{j\in \vect{N}_i^k} p(z_{i,j}^t|\vect{p}_i^t,\vect{X}_j^t) \right]\\
		& \sim \mathcal{N}(\vect{\bar{X}}^{0:k},\vect{\Sigma}_{\bar{X}^{0:k}}^k)
	\end{aligned}	
\end{equation} 
式中$\vect{p}_i^t,\vect{X}_j^t$为$t$时刻，系统内存在相对测量链的第$i$和第$j$个节点的状态值。$p(\vect{p}_i^0)$为节点在初始时刻的先验概率，$p(\vect{p}_{i}^{t}|\vect{p}_i^{t-1},\vect{u}_i^{t-1})$为运动模型~\eqref{eq. ACOTO_motionModel}中表示的状态转移概率，$p(z_{i,j}^t|\vect{p}_i^t,\vect{X}_j^t)$为观测模型~\eqref{eq. ACOTO_observationModel1}中表示的观测数据的置信概率。 下面分别将分别对滤波引擎和平滑引擎的原理进行介绍，并在最后总结使用两类引擎解决协同定位状态估计问题的具体算法。

\BiSection{滤波引擎：基于拓展卡尔曼滤波的协同定位}{Filtering engine for CL}

本节以中心化的拓展卡尔曼滤波方法（Extended Kalman Filter, EKF）为例，介绍贝叶斯滤波方法在多智能体系统协同定位任务中的应用。当存在时变的测量通信拓扑约束时，分析滤波方法用于预测系统未来状态行为的局限和难点。其他类型的非线性递归滤波方法，如无迹卡尔曼滤波，粒子滤波方法等，也可以进行类似地分析。

使用滤波过程中通用的数学表达形式，以$\vect{\bar{X} }(k|k-1)$和$\vect{\Sigma}(k|k-1)$表示系统对$k$时刻所有智能体位置状态的先验估计，用$\vect{\bar{X}}(k|k)$和$\vect{\Sigma}(k|k)$表示后验估计。
则对式~\eqref{eq. ACTCO_MAP}所示的最大后验概率密度，采用EKF进行状态估计，从$k-1$到$k$时刻的预测过程为：
\begin{equation} \label{eq. prioriUpdateEKF}
	\begin{aligned}
		\vect{\bar{X}}(k|k-1) &= \vect{f}[\vect{\bar{X}}(k-1|k-1),\vect{U}(k-1)]\\
		\vect{\Sigma}(k|k-1) &= \vect{F}(k|k-1)\vect{\Sigma}(k-1|k-1)\vect{F}(k|k-1)^T + \vect{R}(k)
	\end{aligned}	
\end{equation} 
式中函数$\vect{f}$为所有智能体状态转移函数\eqref{eq. ACOTO_motionModel}组成的函数集合，即$\vect{f}=\{ f_i | \forall i \in \mathcal{V}_R \}$。 状态转移矩阵 $\vect{F}(k|k-1)$ 为函数$\vect{f}$ 在先验位置 $\vect{\bar{X}}(k|k-1)$ 处的雅可比矩阵，即
\begin{equation} \label{eq. linearizationF_EKF}
	\vect{F}(k|k-1)=\frac{\partial \vect{f}}{\partial \vect{X}}\lvert_{\vect{X}=\vect{\bar{X}}(k|k-1)}
\end{equation}
且$\vect{R} = diag([\vect{R}_1,\cdots,\vect{R}_N]^T)$为所有智能体运动过程的联合噪音协方差矩阵。

当第$k$时刻所有智能体之间执行了相对测量之后，得到所有邻接节点之间的相对测量数据$\vect{Z}(k)=\{ \vect{z}_{i,j}(k) | \forall i,j \in \mathcal{V}, \forall s_{i,j}(k) \in \mathcal{E}(k), i\neq j \}$，则EKF的测量更新过程为：
\begin{equation} \label{eq. measurementUpdateEKF}
	\begin{aligned}
		\vect{\bar{X}}(k|k) &= \vect{\bar{X}}(k|k-1) + \vect{K}(k) [ \vect{Z}(k) - \vect{h}^k(\vect{\bar{X}(k|k-1)}) ] \\
		\vect{\Sigma}(k|k) &= [\vect{I} - \vect{K}(k)\vect{H}(k)]\vect{\Sigma}(k|k-1)
	\end{aligned}
\end{equation}

式中函数$\vect{h}^k$为$k$时刻所有有效的相对测量所组成的联合观测函数，即$\vect{h}^k = \{ h^k_{i,j} | \forall \vect{z}_{i,j}(k) \in \vect{Z}(k) \}$，$h_{i,j}$为第$i$个节点和第$j$个节点的相对观测\eqref{eq. ACOTO_observationModel1}。其中$\vect{K}(k)$为卡尔曼滤波增益矩阵：
\begin{equation} \label{eq. KalmanGainsEKF}
	\vect{K}(k) = \vect{\Sigma}(k|k-1)\vect{H}^T(k)\left[ \vect{H}(k)\vect{\Sigma}(k|k-1)\vect{H}^T(k) + \vect{Q}(k) \right]^{-1}
\end{equation}
$\vect{H}(k)$为联合测量函数$\vect{h}$在预测的先验状态值$\vect{\bar{X}}(k|k-1)$处的线性化雅可比矩阵：
\begin{equation} \label{eq. linearizationH_EKF}
	\vect{H}(k) = \frac{\partial \vect{h}}{\partial \vect{X}} |_{\vect{X} = \vect{\bar{X}}(k|k-1)}
\end{equation}其中联合观测的噪音协方差矩阵为$\vect{Q} = diag([\vect{Q}_{i,j}]),\forall \vect{z}_{i,j}\in \vect{Z}(k)$。

以上为标准的中心化EKF在协同定位任务中对系统状态的估计过程。该流程通过递归调用，可以估计任意$t-1$到$t$时刻系统状态的转移，$\forall t < k$。连续调用$k$次即可获得从$0$时刻到$k$时刻所有节点历史轨迹的估计值，标准的一步EKF滤波引擎的具体使用方法总结为算法~\ref{alg. FilteringEngineEstimation}.

\begin{algorithm}[htbp]
	\SetAlgoLined
	\KwIn{\\ \quad $\vect{\bar{X}}^{k-1|k-1}, \vect{\Sigma}^{k-1|k-1}$: 信念$b(\vect{X}^{k-1})$的高斯分布参数 \\
		\quad $\vect{U}^{k-1}$: ${k-1}$时刻多智能体执行的控制输入 \\
		\quad $\vect{Z}^{k}$: $k$时刻多智能体收集到的观测数据}
	\KwOut{\\ \quad $\vect{\bar{X}}^{k|k}, \vect{\Sigma}^{k|k}$: 信念$b(\vect{X}^{k})$的高斯分布参数 }
	
	\SetKwProg{Fn}{Function}{:}{end}	
	\Fn{ FilteringEngineEKF( \textbf{Input} ) } {
		计算先验估计 $\vect{\bar{X}}^{k|k-1}, \vect{\Sigma}^{k|k-1} \leftarrow$ 式~\eqref{eq. prioriUpdateEKF} \;
		计算雅可比矩阵： $\vect{F}^{k|k-1} \leftarrow $ 式~\eqref{eq. linearizationF_EKF} 以及 $\vect{H}^{k|k-1} \leftarrow$ 式~\eqref{eq. linearizationH_EKF}\;
		计算卡尔曼增益 $\vect{K}^k \leftarrow$ 式~\eqref{eq. KalmanGainsEKF} \;
		计算后验估计 $\vect{\bar{X}}^{k|k}, \vect{\Sigma}^{k|k} \leftarrow$ 式~\eqref{eq. measurementUpdateEKF}\;
		\KwRet{$\vect{\bar{X}}^{k|k}, \vect{\Sigma}^{k|k}$}\;
	}	
	\AlgoBiCaption{状态估计问题的滤波引擎}{The filtering engine for estimation problem}
	\label{alg. FilteringEngineEstimation}
\end{algorithm}

\BiSection{平滑引擎：基于最大后验概率的协同定位}{Smoothing engine for CL}
平滑引擎是寻找合适的历史轨迹$\vect{X}^{0:k}$，使得下面的后验概率最大化，
\begin{equation*} 
	\begin{aligned}
		\vect{\bar{X}}^{0:k} = & \mathop{\arg\max}\limits_{\vect{{X}}^{0:k}} b(\vect{{X}}^{0:k}) = \mathop{\arg\min}\limits_{\vect{{X}}^{0:k}} -\log b(\vect{{X}}^{0:k}) \\
		& = \mathop{\arg\min}\limits_{\vect{{X}}^{0:k}} -\log p(\vect{X}^{0:k}|\vect{Z}^{0:K},\vect{U}^{0:k-1})		
	\end{aligned}
\end{equation*}

将式~\eqref{eq. ACTCO_MAP}中后验概率密度的展开式带入可得，
\begin{equation} \label{eq. ACTCO_smoothing_expanded_MAP}
	\begin{aligned}
		\vect{\bar{X}}^{0:k} &= \mathop{\arg\min}\limits_{\vect{{X}}^{0:k}}  \sum_{t=1}^{k}\sum_{i \in \mathcal{V}}|| \vect{p}_i^{t} - f_i(\vect{p}_i^{t-1},\vect{u}_i^{t-1},0) ||^2_{\vect{R}_i} \\
		&+ \sum_{t=1}^{k}\sum_{i \in \mathcal{V}}\sum_{j\in\vect{{N}}^{t}_i}||z_{i,j}^{t} - h_{ij}(\vect{X}_i^{t},\vect{X}_j^{t},0)||^2_{{\vect{Q}}_{ij}}
	\end{aligned}
\end{equation}
式中采用标准的马氏范数表达式，即$||\vect{e}||^2_{\Sigma} = \vect{e}^T\vect{\Sigma}^{-1}\vect{e}$；$\vect{R}_i,\vect{Q}_{i,j}$为节点运动过程和测量过程的噪音协方差矩阵。在估计问题中，求解上述最小优化问题的典型方法为采用高斯-牛顿(Gauss-Newton, GN) 迭代策略。 GN迭代过程将上式在先验估计 $\vect{\bar{X}}^{0:k}_{(0)}$ 处进行线性化，从而得到一步向量更新$\Delta \vect{\bar{X}}^{0:k}_{(0)}$，然后通过$\vect{\bar{X}}^{0:k}_{(1)} \leftarrow \vect{\bar{X}}^{0:k}_{(0)} + \vect{\bar{X}}^{0:k}_{(0)}$对状态估计值进行更新。该过程需要迭代多次直至收敛。下面以该过程的第一次迭代内容为例，将GN迭代方法详细介绍如下。

GN迭代的重点为将式~\eqref{eq. ACTCO_smoothing_expanded_MAP}中的每一个累加因子在线性点处进行线性化，然后通过最小二乘法则推导出状态向量的一步更新。在第一次迭代时，需要对线性点进行初始化，一类典型的初始化方法是使用已知的初始节点位置$\vect{X}^0$，每一时刻的控制输入$\vect{U}^{0:k-1}$,和节点的运动模型~\eqref{eq. ACOTO_motionModel}，在无噪音的理想环境下递推出一组系统所有节点轨迹的初始估计，即令
\begin{equation} \label{eq. intialEstimatesSmoothing}
	\vect{\bar{X}}^{0:k}_{(0)} = \begin{bmatrix}
		\vect{X}^0_{(0)} \\
		\vect{X}^1_{(0)} \\
		\vdots \\
		\vect{X}^k_{(0)} \\
	\end{bmatrix} = \begin{bmatrix}
		\vect{X}^0_{(0)} \\
		\vect{f}_{1:N}(\vect{X}^0_{(0)}, \vect{U}^{0},0) \\
		\vdots \\
		\vect{f}_{1:N}(\vect{X}^{k-1}_{(0)}, \vect{U}^{k-1},0) \\
	\end{bmatrix}
\end{equation}
式中$\vect{f}_{1:N}:= \{ f_i, \forall i \in \mathcal{V}_R \}$，为所有节点在同一时刻的运动模型组成的向量。

使用上述位置对式~\eqref{eq. ACTCO_smoothing_expanded_MAP}进行线性化，则式~\eqref{eq. ACTCO_smoothing_expanded_MAP}可转变为：
\begin{equation} \label{eq. ACTCO_smooth_MAP_after_linearization}
	\begin{aligned}
		\qquad \mathop{\arg\min}\limits_{\vect{{X}}^{0:k}} 
		&\sum_{t=1}^{L} \sum_{i=1}^{N} \sum_{j\in\vect{\tilde{N}}^{t}_i} ||\vect{H}_{ij}^{t} \Delta \vect{X}_{i,j}^{t} - \bar{\vect{b}}_{ij,h}^{t} ||^2_{\vect{{Q}}_{ij}} \\
		&+ \sum_{t=1}^{L}\sum_{i=1}^{N} ||\Delta \vect{p}^{t}_i - \vect{F}^{t-1}_i\Delta \vect{p}_i^{t-1} - \bar{\vect{b}}_{i,f}^{t}||^2_{\vect{R}_i}
	\end{aligned}
\end{equation}
式中$\Delta \vect{X}_{i,j}^{t}$为稀疏的、按照对应节点分块的向量，其中仅对应于第$i$和第$j$个节点的向量块上存在$\Delta \vect{p}^{t}_j$ 和 $\Delta \vect{p}^{t}_j$，其余的向量块均为零。式中的矩阵$\vect{H}_{ij}^{t}$为在$t$时刻第$i$和第$j$个节点之间测量函数在线性点处的线性雅可比矩阵，$\vect{F}^{t}_i$为第$i$个节点在第$t$时刻运动模型在线性点处的线性雅可比矩阵，即
\begin{align*} 
	\vect{F}^{t-1}_i &= \frac{\partial {f}_i^{t-1}}{\partial \vect{X}} |_{\vect{X} = \vect{\bar{X}}^{0:k}_{(0)}} \\
	\vect{H}_{ij}^{t} &= \frac{\partial {h}_{ij}^{t}}{\partial \vect{X}} |_{\vect{X} = \vect{\bar{X}}^{0:k}_{(0)}} 
\end{align*}
另外式~\eqref{eq. ACTCO_smooth_MAP_after_linearization}中的向量$\bar{\vect{b}}_{ij,h}^{t}$和$\bar{\vect{b}}_{i,f}^{t}$定义如下：
\begin{align}
	\bar{\vect{b}}_{i,f}^{t} &= f_i^{t-1}(\vect{\bar{p}}_i^{t-1},\vect{u}_i^{t-1},0) - \vect{\bar{p}}_i^{t} \\
	\bar{\vect{b}}_{ij,h}^{t} &= z_{i,j}^{t} - h_{ij}^{k+t}(\vect{\bar{p}}_i^{t},\vect{p}_j^{t},0)\label{eq. ACTCO_measurement_residual_smoothing}
\end{align}
式中$\vect{\bar{p}}_i^t$为线性点$\vect{\bar{X}}^{0:k}_{(0)}$中第$t$时刻的状态向量$\vect{\bar{X}}^{t}_{(0)}$里对应于第$i$个节点的状态值。由于第一次迭代中，线性点选取的特殊性，因此此处的$\bar{\vect{b}}_{i,f}^{t} = \vect{0}$是零向量，然而当迭代步数增加，线性点将不再是仅仅依据运动模型递推而来的先验状态，因此在后续的迭代中，$\bar{\vect{b}}_{i,f}^{t} $将不再是零向量。

%\begin{algorithm}[htbp]
%	\SetAlgoLined
%	\KwIn{\\ \quad $\vect{\bar{X}}^{0}, \vect{\Sigma}^{0}$: parametrization of the intial belief $b(\vect{X}^{0})$ \\
%		\quad $\vect{U}^{0:k-1}$: history control actions from time $0$ to $k-1$ \\
%		\quad $\vect{Z}^{0:k}$: history observations from time $0$ to time $k$\\
%		\quad $r_L$: maximum loop times for the GN iteration}
%	\KwOut{\\ \quad $\vect{\bar{X}}^{0:k|k}, \vect{\Sigma}^{0:k|k}$: parameters of estimated distribution for the joint belief $b(\vect{X}^{0:k})$ }
%	
%	\SetKwProg{Fn}{Function}{:}{end}	
%	\Fn{ SmoothingEngine( \textbf{Input} ) } {
%		Compute initial estimates $\vect{\bar{X}}^{0:k}_{(0)}$ using \eqref{eq. intialEstimatesSmoothing}\;
%		\For{$i = 0:r_L-1$}{
%			Form the joint matrix $\vect{A}^{0:k}$ and vector $\vect{\bar{b}}^{0:k}$ using \eqref{eq. smoothing struture of A and b}\;
%			Compute the update vector $\Delta \vect{X}^{0:k}_{i}$ using \eqref{eq. updateDeltaVectorSmoothing}\;
%			Compute new estimates using $\vect{\bar{X}}^{0:k}_{(i+1)} = \vect{\bar{X}}^{0:k}_{(i)} + \Delta \vect{\bar{X}}^{0:k+L}_{(i)}$ and $	\vect{\Sigma}_{\bar{\vect{X}}^{0:k}}^{(i+1)} = \left( (\vect{A}^{0:k})^T\vect{A}^{0:k} \right)^{-1}$\;
%		}
%		$\vect{\bar{X}}^{0:k|k}=\vect{\bar{X}}^{0:k|k}_{r_L}, \vect{\Sigma}^{0:k|k} = \vect{\Sigma}_{\bar{\vect{X}}^{0:k}}^{r_L} $ \;
%		\KwRet{$\vect{\bar{X}}^{0:k|k}, \vect{\Sigma}^{0:k|k}$}\;
%	}
%	
%	\AlgoBiCaption{状态估计问题的平滑引擎}{The smoothing engine for estimation problem}
%	\label{alg. SmoothingEngineEstimation}
%\end{algorithm}

\begin{algorithm}[htbp]
	\SetAlgoLined
	\KwIn{\\ \quad $\vect{\bar{X}}^{0}, \vect{\Sigma}^{0}$: 初始信念$b(\vect{X}^{0})$的高斯分布参数 \\
		\quad $\vect{U}^{0:k-1}$: 从 $0$ 到 $k-1$时刻的历史控制输入 \\
		\quad $\vect{Z}^{0:k}$: 从$0$ 到 $k$时刻的历史观测数据\\
		\quad $r_L$: GN迭代方法的最大迭代次数}
	\KwOut{\\ \quad $\vect{\bar{X}}^{0:k|k}, \vect{\Sigma}^{0:k|k}$: 联合信念$b(\vect{X}^{0:k})$经估计得到的高斯分布参数  }
	
	\SetKwProg{Fn}{Function}{:}{end}	
	\Fn{ SmoothingEngine( \textbf{Input} ) } {
		计算初始的先验估计$\vect{\bar{X}}^{0:k}_{(0)} \leftarrow$ 式~\eqref{eq. intialEstimatesSmoothing} \;
		\For{$i = 0:r_L-1$}{
			组建紧致矩阵$\vect{A}^{0:k}$ 和紧致向量$\vect{\bar{b}}^{0:k} \leftarrow$ 式~\eqref{eq. smoothing struture of A and b}\;
			计算一步更新向量 $\Delta \vect{X}^{0:k}_{i} \leftarrow$ 式~\eqref{eq. updateDeltaVectorSmoothing}\;
			计算更新后的估计参数：\\
			\qquad  $\vect{\bar{X}}^{0:k}_{(i+1)} = \vect{\bar{X}}^{0:k}_{(i)} + \Delta \vect{\bar{X}}^{0:k+L}_{(i)}$; $	\vect{\Sigma}_{\bar{\vect{X}}^{0:k}}^{(i+1)} = \left( (\vect{A}^{0:k})^T\vect{A}^{0:k} \right)^{-1}$\;
		}
		$\vect{\bar{X}}^{0:k|k}=\vect{\bar{X}}^{0:k|k}_{r_L}, \vect{\Sigma}^{0:k|k} = \vect{\Sigma}_{\bar{\vect{X}}^{0:k}}^{r_L} $ \;
		\KwRet{$\vect{\bar{X}}^{0:k|k}, \vect{\Sigma}^{0:k|k}$}\;
	}
	
	\AlgoBiCaption{状态估计问题的平滑引擎}{The smoothing engine for estimation problem}
	\label{alg. SmoothingEngineEstimation}
\end{algorithm}

将式~\eqref{eq. ACTCO_smooth_MAP_after_linearization}中每一个累加因子都整合为一个针对一步更新向量$\Delta \vect{\bar{X}}^{0:k}_{(0)}$的紧致形式，可得
\begin{equation} \label{eq. ACTCO_MAP_compact_matrix_form}
	|| \vect{A}^{0:k} \Delta \vect{X}^{0:k}_{(0)} - \vect{\bar{b}}^{0:k}(\vect{U}^{0:k-1},\vect{Z}^{0:k})||^2
\end{equation}
原式~\eqref{eq. ACTCO_smooth_MAP_after_linearization}中的协方差矩阵被分解并被乘到了范数里面，即$||\vect{e}||_{\vect{\Sigma}}^2 = || \vect{\Sigma}^{-\frac{1}{2}} \vect{e} ||^2$。其他新引入的向量表达式为
\begin{equation} \label{eq. smoothing struture of A and b}
	\vect{A}^{0:k} = \left[ \begin{matrix}
		\vect{F}^{0:k} \\
		\vect{H}^{0:k}
	\end{matrix} \right]\qquad	
	\bar{\vect{b}}^{0:k} =\left[  \begin{matrix}
		\bar{b}_{i,f}^{t}, \forall i \in \mathcal{V}_R, \forall t \le k\\
		\bar{b}_{ij,h}^{k+t}, \forall i,j \in \mathcal{V}_R, \forall t \le k
	\end{matrix} \right]
\end{equation}
式中$\vect{F}^{0:k}$和$\vect{H}^{0:k}$为按照$\Delta \vect{\bar{X}}^{0:k}_{(0)}$向量中每个节点位置顺序，合理排列所有的运动过程线性雅可比矩阵$\vect{R}_i^{-\frac{1}{2}} \left[ \cdots, -\vect{F}_i^{t}, \cdots, \vect{I},\cdots \right], \forall i \in \mathcal{V}_R, \forall t \le k$ 以及所有测量过程的线性雅可比矩阵$\vect{Q}_{i,j}^{-\frac{1}{2}} \vect{H}_{i,j}^t,\forall i,j \in \mathcal{V}_R, \forall t \le k$，所形成的联合向量。

至此，针对紧致形式~\eqref{eq. ACTCO_MAP_compact_matrix_form}，可依据摩尔-彭斯(Moore-Pense)伪逆计算出一步更新向量$\Delta \vect{X}^{0:k}_{0}$,
\begin{equation} \label{eq. updateDeltaVectorSmoothing}
	\Delta \vect{\bar{X}}^{0:k}_{(0)} = \left( (\vect{A}^{0:k})^T\vect{A}^{0:k} \right)^{-1}(\vect{A}^{0:k})^T \bar{\vect{b}}^{0:k}
\end{equation}
则初始的估计轨迹$\vect{X}^{0:k}_{0}$可更新为
\begin{equation}
	\vect{\bar{X}}^{0:k}_{(1)} = \vect{\bar{X}}^{0:k}_{(0)} + \Delta \vect{\bar{X}}^{0:k+L}_{(0)}
\end{equation}
以及当前估计值的估计协方差矩阵可按照如下公式计算：
\begin{equation} 
	\vect{\Sigma}_{\bar{\vect{X}}^{0:k}}^{(1)} = \left( (\vect{A}^{0:k})^T\vect{A}^{0:k} \right)^{-1}
\end{equation} 

以上即为GN迭代方法的一次更新过程，此过程一般需要迭代多次，直至系统历史轨迹的估计值收敛到稳定值为止。基于GN迭代方法的平滑引擎求解方法总结为算法~\ref{alg. SmoothingEngineEstimation}。

另外，本文后一章需要用到对未来状态先验分布~\eqref{eq. ACOTO_priorBeliefGivenControl}的预测。此时可仿照式~\eqref{eq. ACTCO_smoothing_expanded_MAP}的展开形式，将式~\eqref{eq. ACOTO_priorBeliefGivenControl}进行同等处理可得：
\begin{equation} \label{eq. ACTCO_smoothing_expanded_MAP_Priori}
	\begin{aligned}
		\vect{\bar{X}}^{0:k+L|k} &= \mathop{\arg\min}\limits_{\vect{{X}}^{0:k+L|k}}  \sum_{t=1}^{k+L}\sum_{i \in \mathcal{V}}|| \vect{p}_i^{t} - f_i(\vect{p}_i^{t-1},\vect{u}_i^{t-1},0) ||^2_{\vect{R}_i} \\
		&+ \sum_{t=1}^{k}\sum_{i \in \mathcal{V}}\sum_{j\in\vect{{N}}^{t}_i}||z_{i,j}^{t} - h_{ij}(\vect{X}_i^{t},\vect{X}_j^{t},0)||^2_{{\vect{Q}}_{ij}}  
		%+ \sum_{t = k+1}^{k+L} \sum_{i \in \mathcal{V}} || \vect{p}_i^{t} - f_i(\vect{p}_i^{t-1},\vect{u}_i^{t-1},0) ||^2_{\vect{R}_i},
	\end{aligned}
\end{equation}
因此上式所对应的联合雅可比矩阵为：
\begin{equation} \label{eq. smoothing struture of A_Priori}
	\vect{A}^{0:k+L|k} = \left[ \begin{matrix}
		\vect{F}^{0:k} \\
		\vect{H}^{0:k} \\
		\vect{F}^{k+1:k+L|k}
	\end{matrix} \right]
\end{equation}
式中矩阵$\vect{F}^{k+1:k+L|k}$为对应于$ \prod_{t = 1}^{L} p(\vect{X}^{k+t}| \vect{X}^{k+t-1}, \vect{U}^{k+t-1})$部分的运动过程雅可比矩阵。该项在线性化过程中的初始线性点$\vect{\bar{X}}^{k+1:k+L|k}_{(0)}$可参照式~\eqref{eq. intialEstimatesSmoothing}进行计算，即
\begin{equation} \label{eq. intialEstimatesSmoothingPriori}
	\vect{\bar{X}}^{k+1:k+L|k}_{(0)} = \begin{bmatrix}
		\vect{X}^{k+1|k}_{(0)} \\
		\vect{X}^{k+2|k}_{(0)} \\
		\vdots \\
		\vect{X}^{k+L|k}_{(0)} \\
	\end{bmatrix} = \begin{bmatrix}
		\vect{f}_{1:N}(\vect{X}^{k|k}_{(0)}, \vect{U}^{k},0) \\
		\vect{f}_{1:N}(\vect{X}^{k+1|k}_{(0)}, \vect{U}^{k+1},0)\\
		\vdots \\
		\vect{f}_{1:N}(\vect{X}^{k+L-1|k}_{(0)}, \vect{U}^{k+L-1},0) \\
	\end{bmatrix}
\end{equation}
因此，未来系统状态先验分布的均值向量协方差矩阵的计算方法分别为为：
\begin{subequations} \label{eq. SmoothingPriorEstimates}
	\begin{equation}
		\vect{\bar{X}}^{0:k+L|k} = \vect{\bar{X}}^{0:k} \bigcup \vect{\bar{X}}^{k+1:k+L|k}_{(0)}
	\end{equation}
	\begin{equation} 
		\vect{\Sigma}_{\bar{\vect{X}}^{k+1:k+L|k}} = \left( (\vect{A}^{k+1:k+L|k})^T\vect{A}^{k+1:k+L|k} \right)^{-1}
	\end{equation} 
\end{subequations}




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


\setcounter{equation}{0}
\setcounter{section}{0}

\renewcommand\theequation{B-\arabic{equation}}
\renewcommand\thesection{B.\arabic{section}}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


\BiAppChapter{附录~B~ 小节~\ref{subsec. ch5sec3.2}中推论证明}{Appendix~B~ Proof of the Corollaries in Subsection~\ref{subsec. ch5sec3.2}  }%


\BiSection{证明推论~\ref{cor. PROB_dw}}{The proof of corollary~\ref{cor. PROB_dw} } \label{appendix. PROB_dw}
\begin{proof}
	根据式~\eqref{eq.PROB_d_w}中对$d_w$的定义，将该序列的两个相邻项作减法可得：
	\begin{equation*}
		\begin{aligned}
			&d_{w+1} - d_w  \\
			&= \frac{(2\lambda_j)^{-w}}{2}  \sum_{j=1}^2 \left\lbrace \left[ 1-(w+1)b_j^2 \right] (2\lambda_j)^{-1} - (1-wb_j^2) \right\rbrace  \\
			&= \frac{(2\lambda_j)^{-w}}{2}  \sum_{j=1}^2 \left\lbrace  wb_j^2\left[ 1 - (2\lambda_j)^{-1} \right] + (1-b_j^2)(2\lambda_j)^{-1} - 1\right\rbrace
		\end{aligned}
	\end{equation*}
	由于假设$\left[ 1 - (2\lambda_j)^{-1} \right]>0$，因此如果
	\begin{equation*} 
		\begin{aligned}
			w &\ge \frac{1 + (b_j^2 - 1)(2\lambda_j)^{-1}}{ b_j^2\left[ 1 - (2\lambda_j)^{-1} \right]}, \\
			& \ge \frac{1}{b_j^2} + \frac{1}{2\lambda_j - 1}
		\end{aligned}  \forall j \in \{ 1,2 \}
	\end{equation*}
	$d_w$序列相邻两项的差值有
	$$d_{w+1} - d_{w} > 0$$
	即$d_w$序列在$w$大于某个阈值后将持续增加。
	
	另外，从$d_w$的定义可知，当
	\begin{equation}
		w \ge \frac{1}{b_j^2},  \forall j \in \{ 1,2 \} 
	\end{equation}
	时，有$d_w \le 0, \forall k \in \mathbb{R}^+$。因此可知，序列$d_w$在$w$足够大时，随着$w$的变大而增加的负数。因此该序列的极限必然是无限趋近于0。
	
	又因为$d_w$对任意的$w$都有定义，$d_1\neq \infty$，因此必然存在某一元素是整个序列的最大值$\bar{d}$，使得$|d_w|\le \bar{d}, \forall w \in \mathbb{R}^+$。
	
	至此，推论~\ref{cor. PROB_dw}得证。
\end{proof}


\BiSection{证明推论~\ref{cor. PROB_recursive_c_w}}{The proof of corollary~\ref{cor. PROB_recursive_c_w} } \label{appendix. PROB_recursive_c_w}
\begin{proof}
	首先将$\tilde{c}_w$序列的两个相邻项$w$和$w+1$进行展开，
	\begin{equation} \label{eq. PROBappen_expan_c_w}
		w\tilde{c}_w = \left( \tilde{c}_0\tilde{d}_w + \tilde{c}_1\tilde{d}_{w-1} + ... + \tilde{c}_{w-1}\tilde{d}_1\right)
	\end{equation}
	以及
	\begin{equation} \label{eq. PROBappen_expan_c_w+1}
		(w+1) \tilde{c}_{w+1} = \left( \tilde{c}_0\tilde{d}_{w+1} + \tilde{c}_1\tilde{d}_{w} + ... + \tilde{c}_{w}\tilde{d}_1\right)
	\end{equation}
	则将式~\eqref{eq. PROBappen_expan_c_w}的左右两边同时添加一个$\tilde{c}_w$可得，
	\begin{equation} \label{eq. PROBappen_expan_c_w+1_add_onemoreterm}
		(w+1)\tilde{c}_w = \left( \tilde{c}_0\tilde{d}_w + \tilde{c}_1\tilde{d}_{w-1} + ... + \tilde{c}_{w-1}\tilde{d}_1 + \tilde{c}_w \right)
	\end{equation}
	用式~\eqref{eq. PROBappen_expan_c_w+1} 减去式~\eqref{eq. PROBappen_expan_c_w+1_add_onemoreterm}可得，
	\begin{equation*}
		\begin{aligned}
			&(w+1)\left( \tilde{c}_{w+1} - \tilde{c}_{w} \right) \\
			&= \tilde{c}_0 (\tilde{d}_{w+1} - \tilde{d}_{w} ) + ... + \tilde{c}_{w-1} (\tilde{d}_{2} - \tilde{d}_{1} )  +\tilde{c}_w (\tilde{d}_{1} - 1 ).  
		\end{aligned}		
	\end{equation*}
	由于$\tilde{d}_{w+1} = \tilde{d}_{w} = ... = \tilde{d}_{1} = \bar{d}$，因此上式转变为：
	\begin{equation*} 
		\begin{aligned}
			(w+1)&\left( \tilde{c}_{w+1} - \tilde{c}_{w} \right) = (\bar{d} - 1)\tilde{c}_w \\
			\Rightarrow \quad & \tilde{c}_{w+1} = \frac{\bar{d}+w}{w+1} \tilde{c}_{w} 
		\end{aligned}		
	\end{equation*}
	另外，因为$\tilde{c}_0 = c_0 > 0$，因此$\tilde{c}_w > 0, \forall k \in \mathbb{R}^+$。
	
	下面使用数学归纳法证明$\tilde{c}_w$是序列$c_w$的包络上界。
	
	首先当$i=0$时， $c_0 = \tilde{c}_0 \Rightarrow |c_0| \le |\tilde{c}_0|$。 
	
	然后当$i=1$时， $|c_1| = |c_0d_1| \le |c_0| |d^u| = |\tilde{c}_1|.$
	
	假设当$i=w-1$时，满足$|c_{w-1}| \le |\tilde{c}_{w-1}|$。 则对于$i=w$，有
	\begin{equation*} 
		\begin{aligned}
			|c_w| &= |\frac{1}{w} \left( c_0d_w + c_1d_{w-1} + ... + c_{w-1}d_1 \right)| \\
			& \le \frac{1}{w} \left( |c_0||d_w| + |c_1||d_{w-1}| + ... + |c_{w-1}||d_1| \right)\\
			& \le \frac{1}{w} \left( |\tilde{c}_0||\bar{d}| + |\tilde{c}_1||\bar{d}| + ... + |\tilde{c}_{w-1}||\bar{d}| \right) \\
			& = |\tilde{c}_w|
		\end{aligned}	
	\end{equation*}
	因此对任意的$w$,都满足$|c_w| \le |\tilde{c}_w|$。
	
	至此，推论~\ref{cor. PROB_recursive_c_w}得证。
\end{proof}



\BiSection{证明推论~\ref{cor. PROB_e_w^D}}{The proof of corollary~\ref{cor. PROB_e_w^D}} \label{appendix. PROB_e_w^D}

\begin{proof}
	证明新序列$e_w^D$在该推论中各种性质的思路是，使用$c_w$的上界序列$\tilde{c}_w$，依照$e_w^D$序列的定义方式，用同样的定义方法使用$\tilde{c}_w$合成一个$\tilde{e}_w^D$。由于$\tilde{c}_w$是$c_w$的上界序列，该性质可以拓展到具有相同定义方式的序列$\tilde{e}_w^D$和$e_w^D$上，即
	$$|e_k^D| \le |\tilde{e}_k^D|,\forall k \in \mathbb{R}^+$$	
	
	则根据推论~\ref{cor. PROB_recursive_c_w}中$\tilde{c}_w$的递归计算式，当$w>D-1$时，有如下关系式的成立，
	\begin{equation*} \label{eq. tilde_e_k^D and tilde_e_{k+1}^{D}}
		\tilde{e}_{w+1}^D = \frac{\bar{d}+w}{w+1} \frac{\prod_{j=1}^{D}(w+2-j)}{\prod_{j=1}^{D}(w+3-j)}\tilde{e}_{k}^D		
	\end{equation*}
	
	因此当$D=1$，上式可简化为
	\begin{equation*}
		\begin{aligned}
			\tilde{e}_{w+1}^D &= \frac{\bar{d}+w}{w+2} \tilde{e}_{w}^D \\
			& < \tilde{e}_{w}^1 \quad \forall w \ge 0
		\end{aligned}		
	\end{equation*}
	因此当$D=1$时，序列$\tilde{e}_w^D$在随着$w$的增加而逐渐减少。此时$\tilde{e}_w^D$的最大值为$\tilde{e}_0^D = \tilde{c}_0$。
	
	当$D \ge 2$时，根据式~\eqref{eq. new series e_k^D, D>2}的定义方式，可做如下展开，	
	\begin{equation}
		\begin{aligned}
			\tilde{e}_{w+1}^D &= \frac{\bar{d}+w}{w+1} \frac{(w+1)(w+2-D)\prod_{j=2}^{D-1}(w+2-j)}{(w+2)(w+1)\prod_{j=3}^{D}(w+3-j)}\tilde{e}_{w}^D  \\
			& \le \frac{ (\bar{d}+w)(w+2-D) }{ (w+2)(w+1) } \frac{\prod_{j=2}^{D-1}(w+2-j)}{\prod_{j=2}^{D-1}(w+2-j)}\tilde{e}_{w}^D \\
			& = \frac{ (\bar{d}+w)(w+2-D) }{ (w+2)(w+1) }\tilde{e}_{w}^D\quad \forall w \ge D-1
		\end{aligned}		
	\end{equation}
	将系数的分子分母做差可得
	$$
	\begin{aligned}
		&(\bar{d}+w)(w+2-D) - (w+2)(w+1) \\
		&= (\bar{d}-D-1)w +\bar{d}(2-D)-2
	\end{aligned}
	$$
	由于$0 \le \bar{d} \le D$并且$D \ge 2$，因此$\bar{d} -D -1 < 0$，以及$\bar{d}(2-D) \le 0$，因此可知
	\begin{equation} \label{eq. tilde_e_k^D if D>=2}
		\tilde{e}_{w+1}^D < \tilde{e}_{w}^D, \quad \forall w \ge D-1
	\end{equation}
	由此可以总结出$\tilde{e}_w^D$的是有界的且它的极限是有限值，即
	$$\mathop{\lim}_{k \rightarrow \infty} \tilde{e}_{k}^D = \mathit{Const}$$
	此时当$w\le D-2$，根据式~\eqref{eq. new series e_k^D, D>2}中的定义，$\tilde{e}_w^D$的值是随着$w$的增加而递增的，因此在$0 \ge w \le D-2$的区间内，$\tilde{e}_w^D$在$w=D-2$取得最大值。又当$w \ge D-1$时，由上述分析可知，$\tilde{e}_w^D$在$w=D-1$处取得最大值。又$\tilde{e}_{D-2}^D=\tilde{c}_{D-2} < \tilde{c}_{D-1}$，并且$\tilde{e}_{D-1}^D=\frac{\tilde{c}_{D-1}}{D!} \le \tilde{c}_{D-1}$，因此可以总结出，$\tilde{e}_w^D < \tilde{c}_{D-1}, \forall w \in \mathbb{R}^+$。
	
	
	又由于$\tilde{e}_w^D$ 是 $e_w^D$的上界包络，因此$|e_w^D|$的极限也是有限值，且$|e_w^D|<\tilde{c}_{D-1}$。
	
	至此，推论~\ref{cor. PROB_e_w^D}得证。
	
\end{proof}
